#include "pure_pursuit.h"

PurePursuit::PurePursuit(){
    kpLinear_ = 0.5;
}


PurePursuit::~PurePursuit(){

}

// 计算局部目标点的速度
// mc::status PurePursuit::computeTwist(geometry_msgs::PoseStamped goal, geometry_msgs::Twist curr_vel, geometry_msgs::Twist& cmd_vel){
mc::status PurePursuit::computeTwist(mc::TrajectoryPoint2D goal, geometry_msgs::Twist curr_vel,
    geometry_msgs::Twist& cmd_vel){

    mc::status status;

    // 转换为相对机器人位姿的目标点
    mc::TrajectoryPoint2D local_goal = convertGlobalPoseToLocal(goal, robot_pose_);
    double dx = local_goal.pose.translation.x();
    double dy = local_goal.pose.translation.y();

    // 前瞻距离
    double lookhead_distance = hypot(dx, dy);
    double ld_2 = lookhead_distance * lookhead_distance;
    // 横向误差（假设机器人本地坐标的路径点）
    double lateral_error = dy;

    LOG(INFO) << "[PurePursuit] dx: " << dx << ", dy: " << dy 
        << ", lookhead_distance: " << lookhead_distance 
        << ", ld_2: " << ld_2 << ", lateral_error: " << lateral_error;

    if(isPositionReached(robot_pose_, goal)){
        if(!isAngleReached(robot_pose_, goal)){
            rotateToTGoal(robot_pose_, goal, cmd_vel);
        }
    } 
    else{
        // 距离越远，速度越快
        cmd_vel.linear.x = kpLinear_ * lookhead_distance;  // 简单的PID控制
        cmd_vel.linear.y = 0.0;
        cmd_vel.angular.z = 2.0 * lateral_error * cmd_vel.linear.x / ld_2;
    }

    return status;
}